
/**
 * 
 */
package graphics;

import java.awt.Frame;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.io.BufferedWriter;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Set;

import javax.media.opengl.*;
import javax.media.opengl.awt.GLCanvas;

import SRTPlanner.EST;
import SRTPlanner.SRT;
import SRTPlanner.SimpleLocalPlanner;
import SRTPlanner.ThreadedSRT;

import com.jogamp.opengl.util.*;

import simulation.Circle;
import simulation.ConfigGraph;
import simulation.Configuration;
import simulation.Map;
import simulation.Obstacle;
import simulation.Path;
import simulation.Point;
import simulation.Rect;
import simulation.Robot;
import simulation.Simulation;


/**
 * @author aclayton
 */
public class MapDisplay  implements GLEventListener, KeyListener {
	final float[] RobotColor = {1,0,0}; 
	final float[] ObstacleColor = {0,1,0}; 
	
	private Map map;
	private Configuration config;
	private Path path; 
	private ConfigGraph graph;
	private int pathstage;
	
	public MapDisplay(Map map){
		this.map = map;
		this.config = null;
		this.path = null;
		this.graph = null;
	}
	
	public void displayConfiguration(Configuration con){
		config = con;
	}
	
	public void displayPath(Path path){
		pathstage = 0;
		this.path = path;
	}
	
	public void displayConfigGraph(ConfigGraph graph){
		this.graph = graph;
	}
	
	
	public void drawObstacle(GL2 gl, Obstacle ob){
		ArrayList<Point> pts = ob.getPoints();
		
		gl.glBegin(GL.GL_TRIANGLES);
		gl.glColor3f(ObstacleColor[0], ObstacleColor[1], ObstacleColor[2]);
		
		for(Point p : pts){
			gl.glVertex2f((float)p.getX(), (float)p.getY());
		}
		
		gl.glEnd();
	}
	public void drawConfiguration(GL2 gl, Configuration conf){
		for(Circle c : conf.getPositions()){
			System.out.println(c);
			drawCircle(gl,c);
		}
	}
	
	public void drawConfigGraph(GL2 gl, ConfigGraph g){
		Set<Configuration> configurations = g.getConfigurations();
		
		for(Configuration a : configurations){
			drawConfiguration(gl, a);
		}
		
	}
	
	public void drawPath(GL2 gl, Path p){
		Configuration last = null;
		for(Configuration c : p.getPathList()){
			drawConfiguration(gl, c);
			if(last != null){
				gl.glBegin(GL.GL_LINES);
				gl.glColor3f(RobotColor[0], RobotColor[1], RobotColor[2]);
				gl.glVertex2f((float)last.getConfigCenter().getX(), (float)last.getConfigCenter().getY());
				gl.glVertex2f((float)c.getConfigCenter().getX(), (float)c.getConfigCenter().getY());
				gl.glEnd();
			}
			last = c;
		}
	}
	
	public void drawPathAnimated(GL2 gl, Path p){
		if((int)(pathstage/5) >= path.getLength())
			pathstage = 0;
		
		drawConfiguration(gl, path.atStep((int)(pathstage/5)));
		pathstage++;
	}
	
	
	public void drawCircle(GL2 gl, Circle c){
		
		gl.glBegin(GL.GL_TRIANGLE_FAN);
		gl.glColor3f(RobotColor[0], RobotColor[1], RobotColor[2]);
		gl.glVertex2f((float)c.getCenter().getX(), (float)c.getCenter().getY());
		
		for(double angle = 0; angle < 2*Math.PI+.1; angle += Math.PI/6){
			float tempX = (float)(c.getCenter().getX()+c.getRadius()*Math.cos(angle));
			float tempY = (float)(c.getCenter().getY()+c.getRadius()*Math.sin(angle));
			gl.glVertex2f(tempX, tempY);
		}
		
		gl.glEnd();
	}
	
	@Override
	public void display(GLAutoDrawable drawable) {
	    update();
	    render(drawable);
	}
	@Override
	public void reshape(GLAutoDrawable drawable, int x, int y, int w, int h) {

	}
	
	@Override
	public void dispose(GLAutoDrawable drawable){
	}
	
	@Override
	public void init(GLAutoDrawable drawable){
	}
	
	private void update(){
	}
	
	private void render(GLAutoDrawable drawable){
		GL2 gl = drawable.getGL().getGL2();
		
		gl.glMatrixMode(gl.GL_PROJECTION);
		gl.glLoadIdentity();
		gl.glOrtho (0, map.getLength(), 0, map.getWidth(), 0, 1);
		gl.glDisable(gl.GL_DEPTH_TEST);
		gl.glMatrixMode (gl.GL_MODELVIEW);
		gl.glLoadIdentity();
		
		gl.glClearColor(1,1,1, 0);
		gl.glClear(GL.GL_COLOR_BUFFER_BIT);
		
		
		for(Obstacle ob : map.getObstacles())
			drawObstacle(gl, ob);
		
		if(config != null)
			drawConfiguration(gl, config);
		
		
		if(path != null){
			drawPath(gl, path);
			/*
			if(path.hasNext()){
				drawConfiguration(gl, path.next());
			} else {
				path.reset();
			}*/
			
		}
		
		if(graph != null)
			drawConfigGraph(gl, graph);
		//for(Robot rob : map.getRobots())
		//	drawRobot(gl, rob);
			
		//Obstacle obs = Obstacle.createRandomObstacleInBox(0, 0, 2, 2);//new Obstacle(new Point(-1,-1), new Point(0,1), new Point(1,-1));
		//drawObstacle(gl, obs);
		//System.out.println(obs);
		
		//Robot rob = new Robot(new Point(1,1), 1);
		//drawRobot(gl, rob);
	}
	
	public static void main(String[] args) {
		
		if(!(args.length == 2 || args.length == 3)){
			System.out.println("Usage");
			System.out.println("MapDisplay Map mapfile");
			System.out.println("MapDisplay Simulation simulationfile");
			System.out.println("MapDisplay Path simulationfile pathfile");
			System.exit(0);
		}
		
		
        GLProfile glp = GLProfile.getDefault();
        GLCapabilities caps = new GLCapabilities(glp);
        GLCanvas canvas = new GLCanvas(caps);
        
        System.out.println(args[0]);
        Map m = new Map(5,5);
        Simulation sim = new Simulation(m);
        Path p = null;
        if(args[0].equals("Map")){
             try{
             	m.loadFromFile(args[1]);//"C:\\Users\\aclayton\\Documents\\aclayto5\\SensorBasedRobotics\\SensorBasedProject\\testload.txt");
             } catch (FileNotFoundException e) {
             	System.out.println(e);
             	System.exit(0);
             }
        } else if(args[0].equals("Simulation")) {
        	try{
             	sim.LoadFromFile(args[1]);//"C:\\Users\\aclayton\\Documents\\aclayto5\\SensorBasedRobotics\\SensorBasedProject\\testload.txt");
             } catch (Exception e) {
             	System.out.println(e);
             	System.exit(0);
             }
        } else if(args[0].equals("Path")) {
        	sim.LoadFromFile(args[1]);
        	p = Path.createFromFile(sim, args[2]);
        } else {
        	System.exit(0);
        }
        
        /*
        Simulation sim = new Simulation(m);
        sim.createRobots(1, 1);
        Configuration start = Configuration.CreateRandomFreeConfig(sim, sim.getMap().getBoundingBox());
        Configuration end = Configuration.CreateRandomFreeConfig(sim, sim.getMap().getBoundingBox());
        */
        /*
        Rect[] b1 = sim.getMap().getBoundingBox().splitVertically();
        
        EST eTree1 = new EST(sim, b1[0]);
        eTree1.addToTree(sim.getStart());
        EST eTree2 = new EST(sim, b1[1]);
        eTree2.addToTree(sim.getGoal());
        
        eTree1.addRandomConfigurations(5000);
        eTree2.addRandomConfigurations(5000);
        
        SRT srt = new SRT(sim);
        srt.addGraph(eTree1.getGraph());
        srt.addGraph(eTree2.getGraph());
        srt.runSRT();
        
        ConfigGraph graph = srt.getSRTGraph();
        Path p = graph.getAStarPath(sim.getStart(), sim.getGoal());
        
        ArrayList<Configuration> endconnects = graph.getConnected(sim.getGoal());
        */
        
        
        /*
        SimpleLocalPlanner lp = new SimpleLocalPlanner(sim.getMap());
        for(int a = 0; a < 5; a++){
        	Configuration c;
        	do{
        		c = Configuration.CreateRandomFreeConfig(sim, sim.getMap().getBoundingBox());
        	} while(!lp.existsPath(last, c));
        	p.addtoend(c);
        	last = c;
        }*/
        
        //System.out.println(args[0]);
       
        //m.generateRandomObstacles(2);
        //System.out.println(m);
        //m.saveToFile();
        
        final Frame frame = new Frame("AWT Window Test");
        frame.setSize(300, 300);
        frame.add(canvas);
        frame.setVisible(true);
        
        // by default, an AWT Frame doesn't do anything when you click
        // the close button; this bit of code will terminate the program when
        // the window is asked to close
        frame.addWindowListener(new WindowAdapter() {
            public void windowClosing(WindowEvent e) {
                System.exit(0);
            }
        });
        
        MapDisplay mDisplay = new MapDisplay(m);
        //mDisplay.displayConfiguration(sim.getGoal());
        if(p != null)
        	mDisplay.displayPath(p);
        frame.addKeyListener(mDisplay);
        canvas.addGLEventListener(mDisplay);
        
        FPSAnimator animator = new FPSAnimator(canvas, 1);
        animator.add(canvas);
        animator.start();
    }

	@Override
	public void keyPressed(KeyEvent arg0) {
		System.out.println("Key Pressed");
		
	}

	@Override
	public void keyReleased(KeyEvent arg0) {
		// TODO Auto-generated method stub
		System.out.println("Key Pressed");
	}

	@Override
	public void keyTyped(KeyEvent arg0) {
		// TODO Auto-generated method stub
		System.out.println("Key Pressed");
		
	}
}
